/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : sensors.cpp
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/


#include "sensors.h"
#include "i2cmaster.h"
#include "pipo.h"

int16_t sensor_data_int[10]    = {0};      // data from sensors.  3 int from accel(0..2) + 3 int from gyro(3..5)
int16_t sensor_bias[10]        = {0};      // sensor biases
int32_t sensor_mas[10]         = {0};      // sensor moving average star MA*[i]
uint8_t gyroAddr = L3GD20_ADDR;


/*************************************************************************
 read, calibrate, normalize, LP filter  data from sensors
*************************************************************************/
void populateSensorData()
{ 
    int16_t tmp;

    //read XYZ data from L3G4200D - 612us
    // read from register 0x28 + MSb set to auto increment
    i2cRead6(gyroAddr,((0x28) | (1<<7)),(uint8_t *)&sensor_data_int[SEN_GX]);

    // Convert to NED coordinates
    //sensor_data_int[SEN_GX]= (sensor_data_int[SEN_GX]);
    sensor_data_int[SEN_GY]= -(sensor_data_int[SEN_GY]);
    sensor_data_int[SEN_GZ]= -(sensor_data_int[SEN_GZ]);

    //read XYZ data from ADXL345 - 612us
    i2cRead6(ADXL_ADDR, 0x32, (uint8_t *)&sensor_data_int[SEN_AX]);
    tmp = sensor_data_int[SEN_AX];
    sensor_data_int[SEN_AX] =  -sensor_data_int[SEN_AY];
    sensor_data_int[SEN_AY] = -tmp;
    //sensor_data_int[SEN_AZ] = (sensor_data_int[SEN_AZ]);

    //read XYZ data from HMC5883L - 904us
    if(magnetometer)
    {
        if (i2cRead(HMC5883L_ADDR,0x09)& 1 ) // only read when ready
        {
            uint8_t * t = (uint8_t *)&sensor_data_int[SEN_MX];
            i2cRead6(HMC5883L_ADDR, 0x03, t);
            //HMC5883L is weird!  Y and Z are swapped as well!
            //we need to swap bytes, and do the following swaps:
            //y <= -z
            //z <= y
            //x <= -x
            //HMC5883L is big endian -> swap bytes

            SWAP(t[0], t[1]);
            SWAP(t[2], t[3]);
            SWAP(t[4], t[5]);

            tmp = sensor_data_int[SEN_MY];
            sensor_data_int[SEN_MX] = -sensor_data_int[SEN_MX]; //rotate 180 degrees - HMC5883L
            sensor_data_int[SEN_MY] = sensor_data_int[SEN_MZ];
            sensor_data_int[SEN_MZ] = -tmp;
        }
    }


    //Moving average filter
    //http://www.daycounter.com/LabBook/Moving-Average.phtml
    //MA*[i]= MA*[i-1] +X[i] - MA*[i-1]/N
    //where MA* is the moving average*N.
    //MA[i]= MA*[i]/N
    for(uint8_t j=0; j<9; j++) //60.4us
    {
        sensor_data_int[j] <<= NPOINT_AVERAGE; //increase accuracy
        sensor_mas[j] += sensor_data_int[j] - (sensor_mas[j] >> NPOINT_AVERAGE);// /NPOINT_AVERAGE=2 -> 2^2 - averaging over 4 points;
        sensor_data_int[j] = (sensor_mas[j] >> (2*NPOINT_AVERAGE)); // / (NPOINT_AVERAGE*NPOINT_FILTER_ACCURACY);
    }

    for(uint8_t j=0; j<9; j++)  //10us
        sensor_data_int[j] -= sensor_bias[j];
}


/*************************************************************************
 Calibrate Gyro
*************************************************************************/
void calibrateGyro()
{
    int32_t accumulator[3] = {0};
    uint16_t counter;

    for (counter=0; counter<(CALIB_SAMPLES*4); counter++) //first loop - read samples to make sure reading is up to date
    {
        LED_GREEN_FLIP;  // flash GREEN led to signify initialization
        uint16_t cg25ms = g25msec + 1;
        populateSensorData();

        while (cg25ms != g25msec);  // wait for next 25ms period
    }

    for (counter=0; counter<CALIB_SAMPLES; counter++)
    {
        LED_RED_FLIP;  // flash red led to signify calibration
        uint16_t cg25ms = g25msec + 4; // current timer - make sure we don't read more than once every 100ms
        populateSensorData();

        for(uint8_t i=0; i<3; i++)
            accumulator[i] += sensor_data_int[i+SEN_GX];

        while (cg25ms != g25msec);  // wait for next 100ms period - one sample every 100ms
    }

    if(counter>0)
    {
        for(uint8_t i=0; i<3; i++)
            sensor_bias[i+SEN_GX] = accumulator[i]/counter;
    }
}

/*************************************************************************
 Calibrate Compass
*************************************************************************/
void calibrateCompass()
{
    // measure min and max for each axis until interrupted
    // when done
    // bias[axis] = (min+max)/2
    int16_t min[3] = {0};
    int16_t max[3] = {0};
    uint16_t sampleCount = 0;

    LED_RED_OFF;
    LED_GREEN_ON;
    while (sampleCount < 200) // 20 seconds of calibration
    {
        LED_RED_FLIP;  // flash red led to signify calibration
        LED_GREEN_FLIP;  // flash red led to signify calibration
        uint16_t cg25ms = g25msec + 4; // current timer - make sure we don't read more than once every 100ms
        populateSensorData();

        for(uint8_t i=0; i<3; i++)
        {
            if(min[i]>sensor_data_int[i+SEN_MX]) min[i]=sensor_data_int[i+SEN_MX];
            if(max[i]<sensor_data_int[i+SEN_MX]) max[i]=sensor_data_int[i+SEN_MX];
        }

        while (cg25ms != g25msec);  // wait for next 100ms period - one sample every 100ms
        sampleCount++;
    }

    //once done - calculate biases and save to EEPROM
    for(uint8_t i=0; i<3; i++)
        sensor_bias[i+SEN_MX] = (min[i] + max[i]) / 2;

    //Save to EEPROM
}


/*************************************************************************
 set up ADXL345
*************************************************************************/
uint8_t adxl_init()
{
    if(!i2cDeviceReady(ADXL_ADDR))
        return 0;

    if(i2cRead(ADXL_ADDR,0x00)!=ADXL_ID)
        return 0;

    i2cWrite(ADXL_ADDR, 0x2C, 0x0C);   //BW_Rate register  //ODR = 400Hz, BW = 200Hz
    i2cWrite(ADXL_ADDR, 0x31, 0x10);   // DATA_FORMAT register // set the "full rez" bit +-16g
    i2cWrite(ADXL_ADDR, 0x2D, (1<<3)); //PWER_CTL register //Start measure -> bit 3 on.

    return 1;
}

/*************************************************************************
 set up L3G4200D
*************************************************************************/
uint8_t l3g4200d_init()
{
    if(i2cDeviceReady(L3GD20_ADDR)) //it's a L3GD20
    {
        gyroAddr = L3GD20_ADDR;
        if(i2cRead(L3GD20_ADDR,0x0F)!=L3GD20_ID)  //id still needs to match
            return 0;
    }
    else  // maybe it's L3G4200D
    {
        gyroAddr = L3G4200D_ADDR;
        if(!i2cDeviceReady(L3GD20_ADDR))
            return 0; // doesn't answer - not valid

        if(i2cRead(L3G4200D_ADDR,0x0F)!=L3G4200D_ID)
            return 0; // ID should match too
    }

    i2cWrite(gyroAddr, 0x21, 0x34); //CTRL_REG2
    i2cWrite(gyroAddr, 0x23, 0xA0); //CTRL_REG4 -
    //BDU = 1: output registers not updated until MSB and LSB reading
    //BLE = 0: Data LSB
    //FS1-FS0 = 10: 2000 dps
    i2cWrite(gyroAddr, 0x24, 0x13); //CTRL_REG5
    i2cWrite(gyroAddr, 0x20, 0xFF); //CTRL_REG1

    return 1;
}


/*************************************************************************
 set up HMC5883L or HMC5883L
*************************************************************************/
uint8_t HMC5883L_init()
{
    if(!i2cDeviceReady(HMC5883L_ADDR))
        return 0;

    if(i2cRead(HMC5883L_ADDR,0x0A)!=HMC5883L_ID1)  // check id
        return 0;

    i2cWrite(HMC5883L_ADDR,0x00,0x70); // 8-average, 15 Hz default, normal measurement
    i2cWrite(HMC5883L_ADDR,0x01,0xA0); // 4.7Gauss
    i2cWrite(HMC5883L_ADDR,0x02,0x00); // mode register - continuous measurement - clear md1 bit

    return 1;
}


